#ifndef __SENSOR_HANDLE_H__
#define __SENSOR_HANDLE_H__

#include <mc_ros/sensors/sensor_config.h>
#include <mc_ros/visualization/perception_visualization.h>
#include <geometry_msgs/TransformStamped.h>
// #include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>

#include "core/robot_model/robot_model_base.h"
#include "motion_controller/perception/sensors/convert/msg_convert.h"
#include "motion_controller/perception/sensors/lidar_2d_undistortion.h"
#include "motion_controller/perception/sensors/pointcloud_filter.h"
#include "motion_controller/perception/sensors/pointcloud_fusion.h"
#include "motion_controller/perception/sensors/depth2pointcloud.h"

// 激光雷达数据处理流程
struct LidarProcessor{
    Lidar2DUndistortion lidar_2d_undistortion;
    PointCloudFilter pcl_filter;
    PerceptionVisualization visualization;
};


// 深度相机数据处理流程
struct DepthImageProcessor{
    Depth2PointCloud depth2pointcloud;
    PointCloudFilter pcl_filter;
    PerceptionVisualization visualization;
};


class SensorHandle{
    public:
        SensorHandle(std::shared_ptr<SensorConfig> config_ptr);
        ~SensorHandle() = default;

        // 发布TF变化信息
        void publishStaticTF(geometry_msgs::TransformStamped transform_stamped);
        void publishStaticTF(string sensor_frame, SensorTF sensor_tf);
        // 发布传感器静态TF变换
        void publishSensorsStaticTF();

        // 初始化订阅者
        void initSubscriber();
        // 处理雷达类型消息
        void HandleLaserScanMessage(SensorParams sensor_params, const sensor_msgs::LaserScan::ConstPtr& msg);
        // 处理IMU数据
        void HandleIMUMessage(SensorParams sensor_params, const sensor_msgs::Imu::ConstPtr& msg);
        // 处理里程计数据
        void HandleOdometryMessage(SensorParams sensor_params, const nav_msgs::Odometry::ConstPtr& msg);
        // 处理深度相机图像数据
        void HandleCameraImageMessage(SensorParams sensor_params, const sensor_msgs::Image::ConstPtr& msg);
        // 处理深度相机信息数据
        void HandleCameraInfoMessage(SensorParams sensor_params, const sensor_msgs::CameraInfo::ConstPtr& msg);

        // 处理深度相机数据
        vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> DepthCameraHandle();
        // 处理激光雷达数据
        vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Lidar2DHandle();

        // 点云融合
        pcl::PointCloud<pcl::PointXYZ>::Ptr fusionPointCloud(vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clouds);
        // 点云投影
        void projectPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);

    private:
        ros::NodeHandle nh_;
        // tf2_ros::TransformBroadcaster tf_broadcaster_;
        tf2_ros::StaticTransformBroadcaster static_broadcaster_;
        image_transport::ImageTransport image_transport_;
        
        std::shared_ptr<SensorConfig> sensor_config_ptr_;
        std::unordered_map<int, ros::Subscriber> sub_list_;
        std::unordered_map<int, image_transport::Subscriber> camera_sub_list_;
        std::unordered_map<int, LaserScanData> multi_lidar_cache_;
        std::unordered_map<int, ImageData> multi_image_cache_;
        nav_msgs::Odometry::Ptr odom_cache_;
        sensor_msgs::Imu::Ptr imu_cache_;

        std::unordered_map<int, LidarProcessor>  lidar_processor_list_;  //雷达处理流程列表
        std::unordered_map<int, DepthImageProcessor>  depth_processor_list_;  //深度相机数据处理流程列表
        PointCloudFusion pcl_fusion_;
        PointCloudFilter pcl_filter_;
};

#endif